#include <iostream>
#include "msckf.h"
#include <opencv2/core/core.hpp>
#include <chrono>
#include <thread>

#include <System.h>
#include <unistd.h>
#include <algorithm>
#include <fstream>
#include <atomic>



#include "corner_detector.h"




#if 1
//Camera Parameters file
#define CAMERA_PARAMTERS_FILE_PATH		"/home/wenyi/workspace/msckf-g2o-slam/EuRoC_data_source_mono/EuRoC.yaml"

//image timestamp file
#define IMAGE_TIMESTAMP_FILE_PATH		"/home/wenyi/workspace/msckf-g2o-slam/EuRoC_data_source_mono/EuRoC_TimeStamps/V201.txt"

#define EUROC_DATA_LEFT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_01_easy/mav0/cam0/data"
#define EUROC_DATA_RIGHT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_01_easy/mav0/cam1/data"
#define EUROC_DATA_IMU_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_01_easy/mav0/imu0/data.csv"
#elif 0
//Camera Parameters file
#define CAMERA_PARAMTERS_FILE_PATH		"/home/wenyi/workspace/msckf-g2o-slam/EuRoC_data_source_mono/EuRoC.yaml"

//image timestamp file
#define IMAGE_TIMESTAMP_FILE_PATH		"/home/wenyi/workspace/msckf-g2o-slam/EuRoC_data_source_mono/EuRoC_TimeStamps/V202.txt"

#define EUROC_DATA_LEFT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_02_medium/mav0/cam0/data"
#define EUROC_DATA_RIGHT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_02_medium/mav0/cam1/data"
#define EUROC_DATA_IMU_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_02_medium/mav0/imu0/data.csv"
#else
//Camera Parameters file
#define CAMERA_PARAMTERS_FILE_PATH		"/home/wenyi/workspace/msckf-g2o-slam/EuRoC_data_source_mono/EuRoC.yaml"

//image timestamp file
#define IMAGE_TIMESTAMP_FILE_PATH		"/home/wenyi/workspace/msckf-g2o-slam/EuRoC_data_source_mono/EuRoC_TimeStamps/V203.txt"

#define EUROC_DATA_LEFT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_03_difficult/mav0/cam0/data"
#define EUROC_DATA_RIGHT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_03_difficult/mav0/cam1/data"
#define EUROC_DATA_IMU_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_03_difficult/mav0/imu0/data.csv"
#endif

//VocFile
#define EUROC_VOC_FILE_PATH				"/home/wenyi/workspace/msckf-g2o-slam/Vocabulary/ORBvoc.txt"



using namespace std;
using namespace cv;

//----------------------------orb_slam2 main test----------------------


void LoadImages(const string &strImagePath, const string &strPathTimes,
                vector<string> &vstrImages, vector<double> &vTimeStamps);




int main_orb_slam2_test()
{
	printf("start orb-slam2......\n");

	 // Retrieve paths to images
    vector<string> vstrImageFilenames;
    vector<double> vTimestamps;
    LoadImages(EUROC_DATA_LEFT_FILE_PATH, IMAGE_TIMESTAMP_FILE_PATH, vstrImageFilenames, vTimestamps);

    int nImages = vstrImageFilenames.size();

    if(nImages<=0)
    {
        cerr << "ERROR: Failed to load images" << endl;
        return 1;
    }

	// Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(EUROC_VOC_FILE_PATH, CAMERA_PARAMTERS_FILE_PATH,ORB_SLAM2::System::MONOCULAR,true);

    // Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl;

	// Main loop
    cv::Mat im;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file
        im = cv::imread(vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];

        if(im.empty())
        {
            cerr << endl << "Failed to load image at: "
                 <<  vstrImageFilenames[ni] << endl;
            return 1;
        }

#if	1
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif

        // Pass the image to the SLAM system
        SLAM.TrackMonocular(im,tframe);

#if 1
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif

        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;

        // Wait to load the next frame
        double T=0;
        if(ni<nImages-1)
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestamps[ni-1];

        if(ttrack<T)
            usleep((T-ttrack)*1e6);
    }

	// Stop all threads
    SLAM.Shutdown();

    // Tracking time statistics
    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages; ni++)
    {
        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
    cout << "mean tracking time: " << totaltime/nImages << endl;

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

	return 0;
}




void LoadImages(const string &strImagePath, const string &strPathTimes,
                vector<string> &vstrImages, vector<double> &vTimeStamps)
{
    ifstream fTimes;
    fTimes.open(strPathTimes.c_str());
    vTimeStamps.reserve(5000);
    vstrImages.reserve(5000);
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            vstrImages.push_back(strImagePath + "/" + ss.str() + ".png");
            double t;
            ss >> t;
            vTimeStamps.push_back(t/1e9);

        }
    }
}





//--------------------msckf main test-----------------

void LoadIMU(const string &strImuPath, vector<double> &vTimeStamps, vector<cv::Point3f> &vAcc, vector<cv::Point3f> &vGyro);

int main()
{
	printf("msckf mono test\n");
	msckf_mono::MSCKF<float> msckf_;

	//load image timestamps

	// Retrieve paths to images
    vector<string> vstrImageFilenames;
    vector<double> vTimestamps;
    LoadImages(EUROC_DATA_LEFT_FILE_PATH, IMAGE_TIMESTAMP_FILE_PATH, vstrImageFilenames, vTimestamps);

    int nImages = vstrImageFilenames.size();
	cout << "nImages size:" << nImages << endl;

    if(nImages<=0)
    {
        cerr << "ERROR: Failed to load images" << endl;
        return 1;
    }


	//laod imu date
	vector<cv::Point3f> vAcc, vGyro;
    vector<double> vTimestampsImu;

	LoadIMU(EUROC_DATA_IMU_FILE_PATH, vTimestampsImu, vAcc, vGyro);

	int nImu = vTimestampsImu.size();

	cout << "nImu size:" << nImu << endl;


	if(nImu <=0)
	{
		cerr << "ERROR: Failed to load imu" << endl;
        return 2;
	}

	std::vector<std::tuple<double, msckf_mono::imuReading<float>>> imu_queue_;

	// Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

	double prev_imu_time_;
	prev_imu_time_ = 0.0f;
	std::atomic<bool> imu_calibrated_;
	imu_calibrated_ = false;

	double stand_still_time_;
    double done_stand_still_time_;

	stand_still_time_ = 8.0f;


	msckf_mono::imuState<float> init_imu_state_;
	int state_k_;
	msckf_mono::Camera<float> camera_;
    msckf_mono::noiseParams<float> noise_params_;
    msckf_mono::MSCKFParams<float> msckf_params_;


	std::shared_ptr<corner_detector::TrackHandler> track_handler_;

	msckf_mono::Matrix3<float> R_imu_cam_;
    msckf_mono::Vector3<float> p_imu_cam_;


	msckf_mono::Matrix3<float> R_cam_imu_;
    msckf_mono::Vector3<float> p_cam_imu_;

	cv::Mat K_;
	cv::Mat dist_coeffs_;
	std::string distortion_model_ = "radtan";

	int n_grid_cols_;
	int n_grid_rows_;
	float ransac_threshold_;

	//load_parameters
	{
		K_ = cv::Mat::eye(3,3,CV_32F);
		K_.at<float>(0,0) = 458.654f;
		K_.at<float>(1,1) = 457.296f;
		K_.at<float>(0,2) = 367.215f;
		K_.at<float>(1,2) = 248.375f;

		dist_coeffs_ = cv::Mat::zeros(4,1,CV_32F);
		dist_coeffs_.at<float>(0) = -0.28340811f;
		dist_coeffs_.at<float>(1) = 0.07395907f;
		dist_coeffs_.at<float>(2) = 0.00019359f;
		dist_coeffs_.at<float>(3) = 1.76187114e-05;

		msckf_mono::Matrix4<float> T_cam_imu;

		T_cam_imu(0,0) = 0.0148655429818f;
		T_cam_imu(0,1) = -0.999880929698f;
		T_cam_imu(0,2) = 0.00414029679422f;
		T_cam_imu(0,3) = -0.021640145497f;

		T_cam_imu(1,0) = 0.999557249008f;
		T_cam_imu(1,1) = 0.0149672133247f;
		T_cam_imu(1,2) = 0.025715529948f;
		T_cam_imu(1,3) = -0.064676986768f;

		T_cam_imu(2,0) = -0.0257744366974f;
		T_cam_imu(2,1) = 0.00375618835797f;
		T_cam_imu(2,2) = 0.999660727178f;
		T_cam_imu(2,3) = 0.009810730590f;

		T_cam_imu(3,0) = 0;
		T_cam_imu(3,1) = 0;
		T_cam_imu(3,2) = 0;
		T_cam_imu(3,3) = 1;

		R_cam_imu_ =  T_cam_imu.block<3,3>(0,0);
		p_cam_imu_ =  T_cam_imu.block<3,1>(0,3);

		R_imu_cam_ = R_cam_imu_.transpose();
		p_imu_cam_ = R_imu_cam_ * (-1. * p_cam_imu_);

		// setup camera parameters
		camera_.f_u = K_.at<float>(0,0);
		camera_.f_v = K_.at<float>(1,1);
		camera_.c_u = K_.at<float>(0,2);
		camera_.c_v = K_.at<float>(1,2);

		camera_.q_CI = msckf_mono::Quaternion<float>(R_cam_imu_).inverse(); // TODO please check it
		camera_.p_C_I = p_cam_imu_;

		n_grid_rows_ = 8;
		n_grid_cols_ = 8;

		ransac_threshold_ = 0.000002;
		float feature_cov;
		feature_cov = 7;
		Eigen::Matrix<float,12,1> Q_imu_vars;
		float w_var, dbg_var, a_var, dba_var;
		w_var = 1e-5;
		dbg_var = 3.6733e-5;
		a_var = 1e-3;
		dba_var = 7e-4;

		Q_imu_vars << w_var, 	w_var, 	w_var,
						dbg_var,dbg_var,dbg_var,
						a_var,	a_var,	a_var,
						dba_var,dba_var,dba_var;

		Eigen::Matrix<float,15,1> IMUCovar_vars;
		float q_var_init, bg_var_init, v_var_init, ba_var_init, p_var_init;
		q_var_init = 1e-5;
		bg_var_init = 1e-2;
		v_var_init = 1e-2;
		ba_var_init = 1e-2;
		p_var_init = 1e-12;

		IMUCovar_vars << q_var_init, q_var_init, q_var_init,
						bg_var_init,bg_var_init,bg_var_init,
						v_var_init, v_var_init, v_var_init,
						ba_var_init,ba_var_init,ba_var_init,
						p_var_init, p_var_init, p_var_init;
		// Setup noise parameters
		noise_params_.initial_imu_covar = IMUCovar_vars.asDiagonal();
		noise_params_.Q_imu = Q_imu_vars.asDiagonal();
		noise_params_.u_var_prime = pow(feature_cov/camera_.f_u,2);
		noise_params_.v_var_prime = pow(feature_cov/camera_.f_v,2);

		msckf_params_.max_gn_cost_norm = 11;
		msckf_params_.max_gn_cost_norm = pow(msckf_params_.max_gn_cost_norm/camera_.f_u, 2);
		msckf_params_.translation_threshold = 0.05;
		msckf_params_.min_rcond = 3e-12;
		msckf_params_.redundancy_angle_thresh = 0.005;
		msckf_params_.redundancy_distance_thresh = 0.05;
		msckf_params_.max_track_length = 1000;
		msckf_params_.min_track_length = 3;
		msckf_params_.max_cam_states = 20;
	}

	//setup_track_handler
	{
		track_handler_.reset( new corner_detector::TrackHandler(K_, dist_coeffs_, distortion_model_) );
		track_handler_->set_grid_size(n_grid_rows_, n_grid_cols_);
		track_handler_->set_ransac_threshold(ransac_threshold_);
	}

	// Main loop
    cv::Mat im;
	for(int ni=0; ni<nImages; ni++)
	{
        // Read image from file
        im = cv::imread(vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];
		//printf("image time:%lf\n", tframe);

        if(im.empty())
        {
            cerr << endl << "Failed to load image at: "
                 <<  vstrImageFilenames[ni] << endl;
            return 1;
        }

		cv::Mat imGray;
		cv::cvtColor(im, imGray, cv::COLOR_BGR2GRAY);

		std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
		//msckf code
		//imuCallback
		for(int i=ni*10; i<(ni+1)*10; i++)
		{
			//printf("imu time:%lf\n", vTimestampsImu[i]);
			double cur_imu_time = vTimestampsImu[i];
			if(prev_imu_time_ == 0.0)
			{
      			prev_imu_time_ = cur_imu_time;
      			done_stand_still_time_ = cur_imu_time + stand_still_time_;
      			continue;
    		}

			msckf_mono::imuReading<float> current_imu;

    		current_imu.a[0] = vAcc[i].x;
    		current_imu.a[1] = vAcc[i].y;
    		current_imu.a[2] = vAcc[i].z;

    		current_imu.omega[0] = vGyro[i].x;
    		current_imu.omega[1] = vGyro[i].y;
    		current_imu.omega[2] = vGyro[i].z;

			current_imu.dT = cur_imu_time - prev_imu_time_;

    		imu_queue_.emplace_back(cur_imu_time, current_imu);

    		prev_imu_time_ = cur_imu_time;
		}


		//imageCallback
		{
			double cur_image_time = tframe;

			if(!imu_calibrated_)
			{
				if(imu_queue_.size() % 100 == 0)
				{
					printf("Has %ld readings\n", imu_queue_.size());
				}

				if(prev_imu_time_ > done_stand_still_time_)
				{
					//initialize_imu
					{
						Eigen::Vector3f accel_accum;
						Eigen::Vector3f gyro_accum;
						int num_readings = 0;

						accel_accum.setZero();
						gyro_accum.setZero();

						for(const auto& entry : imu_queue_)
						{
							auto imu_time = std::get<0>(entry);
							auto imu_reading = std::get<1>(entry);

							accel_accum += imu_reading.a;
							gyro_accum += imu_reading.omega;
							num_readings++;
						}

						Eigen::Vector3f accel_mean = accel_accum / num_readings;
						Eigen::Vector3f gyro_mean = gyro_accum / num_readings;

						init_imu_state_.b_g = gyro_mean;
						init_imu_state_.g << 0.0, 0.0, -9.81;
						init_imu_state_.q_IG = Quaternion<float>::FromTwoVectors(-init_imu_state_.g, accel_mean);

						init_imu_state_.b_a = init_imu_state_.q_IG*init_imu_state_.g + accel_mean;

						init_imu_state_.p_I_G.setZero();
						init_imu_state_.v_I_G.setZero();
						const auto q = init_imu_state_.q_IG;
					}

					imu_calibrated_ = true;
					imu_queue_.clear();

					//setup_msckf
					{
						state_k_ = 0;
						msckf_.initialize(camera_, noise_params_, msckf_params_, init_imu_state_);
					}
				}
			}
			else
			{
				std::vector<msckf_mono::imuReading<float>> imu_since_prev_img;
				imu_since_prev_img.reserve(10);

				// get the first imu reading that belongs to the next image
				auto frame_end = std::find_if(imu_queue_.begin(), imu_queue_.end(),
						[&](const auto& x){return std::get<0>(x) > cur_image_time;});

				std::transform(imu_queue_.begin(), frame_end,
					std::back_inserter(imu_since_prev_img),
        			[](auto& x){return std::get<1>(x);});

				imu_queue_.erase(imu_queue_.begin(), frame_end);

				for(auto& reading : imu_since_prev_img)
				{
					msckf_.propagate(reading);
					msckf_mono::Vector3<float> gyro_measurement = R_imu_cam_ * (reading.omega - init_imu_state_.b_g);
					track_handler_->add_gyro_reading(gyro_measurement);
				}

				track_handler_->set_current_image(imGray, cur_image_time );

				std::vector<msckf_mono::Vector2<float>,Eigen::aligned_allocator<msckf_mono::Vector2<float>>> cur_features;
				corner_detector::IdVector cur_ids;
				track_handler_->tracked_features(cur_features, cur_ids);

				std::vector<msckf_mono::Vector2<float>,Eigen::aligned_allocator<msckf_mono::Vector2<float>>> new_features;
				corner_detector::IdVector new_ids;
				track_handler_->new_features(new_features, new_ids);

				msckf_.augmentState(state_k_, (float)cur_image_time);
				msckf_.update(cur_features, cur_ids);
				msckf_.addFeatures(new_features, new_ids);
				msckf_.marginalize();
				msckf_.pruneEmptyStates();
			}
		}


		std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();

		double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;

        // Wait to load the next frame
        double T=0;
        if(ni<nImages-1)
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestamps[ni-1];

        if(ttrack<T)
            usleep((T-ttrack)*1e6);
	}

}
void LoadIMU(const string &strImuPath, vector<double> &vTimeStamps, vector<cv::Point3f> &vAcc, vector<cv::Point3f> &vGyro)
{
    ifstream fImu;
    fImu.open(strImuPath.c_str());
    vTimeStamps.reserve(5000);
    vAcc.reserve(5000);
    vGyro.reserve(5000);

    while(!fImu.eof())
    {
        string s;
        getline(fImu,s);
        if (s[0] == '#')
            continue;

        if(!s.empty())
        {
            string item;
            size_t pos = 0;
            double data[7];
            int count = 0;
            while ((pos = s.find(',')) != string::npos) {
                item = s.substr(0, pos);
                data[count++] = stod(item);
                s.erase(0, pos + 1);
            }
            item = s.substr(0, pos);
            data[6] = stod(item);

            vTimeStamps.push_back(data[0]/1e9);
            vAcc.push_back(cv::Point3f(data[4],data[5],data[6]));
            vGyro.push_back(cv::Point3f(data[1],data[2],data[3]));
        }
    }
}

